#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QDebug>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>



MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    initUI();
    m_set_vel_ = 0.0;
    m_set_angle_ = 0.0;
    id1_ = startTimer(20);

    update_time_ = new QTimer();
    connect(update_time_,SIGNAL(timeout()),this,SLOT(time_update()));
    connect(ui->checkBox_rover_enable, SIGNAL(stateChanged(int)), this, SLOT(onStateChanged(int)));

    m_point_cloud_viewer_ = new CCWindow(ui->widget);

}

MainWindow::~MainWindow()
{
    qDebug()<<"destroy Main!";
    rosClose();
    delete ui;
}

bool MainWindow::rosInit(const std::string &master_url, const std::string &host_url)
{
    std::map<std::string,std::string> remappings;
    remappings["__master"] = master_url;
    remappings["__hostname"] = host_url;
    ros::init(remappings,"robot");
    if (!ros::master::check())
    {
        return false;
    }
    ros::start(); // explicitly needed since our nodehandle is going out of scope.
    ros::NodeHandle n;

    /******************************************
     * ros node
     * ****************************************/
    //sub
    imu_subscriber_ = n.subscribe("/BMI088", 100, &MainWindow::imuCallback, this);
    lidar_subscriber_ = n.subscribe("/lidar_pointcloud", 10, &MainWindow::lidarCallback, this);
    cur_twist_sub_ = n.subscribe("/current_velocity", 1, &MainWindow::callbackNavTwistRaw, this);

    //pub
    control_mode_pub_ = n.advertise<std_msgs::String>("/nav_control", 1);
    control_pub_ = n.advertise<geometry_msgs::TwistStamped>("/manual/twist_raw", 1);
    return true;
}

void MainWindow::rosClose()
{
    if(ros::isStarted())
    {
        ros::shutdown(); // explicitly needed since we use ros::start();
        ros::waitForShutdown();
    }
}

void MainWindow::timerEvent(QTimerEvent *event)
{
    if (ros::ok())
    {
        setControlData(m_set_vel_, m_set_angle_);
        ros::spinOnce();
    }
}
void MainWindow::time_update()
{
    qDebug() <<"update"<< endl;
}

void MainWindow::on_button_connect_clicked()
{
    if(ui->button_connect->text().toStdString() == "Connect"){
        if(rosInit(ui->lineEdit_ros_master->text().toStdString(), ui->lineEdit_ros_ip->text().toStdString())){
            ui->button_connect->setText("Close");
            ui->lineEdit_ros_master->setReadOnly(true);
            ui->lineEdit_ros_ip->setReadOnly(true);
            ros_flag_ = true;
            qDebug()<<"ros init ok!";
        }else{
            QMessageBox::warning(this, "warning", tr("ros init fail!"));
            qDebug()<<"ros init fail!";
        }
    }else if(ui->button_connect->text().toStdString() == "Close"){
        rosClose();
        ros_flag_ = false;
        ui->button_connect->setText("Connect");
        ui->lineEdit_ros_master->setReadOnly(false);
        ui->lineEdit_ros_ip->setReadOnly(false);
        qDebug()<<"ros close!";
    }
}

void MainWindow::on_button_up_clicked()
{
    if(ui->checkBox_rover_enable->checkState() == Qt::Unchecked)
        return;
    if(m_set_vel_ < 1.0)
    {
        m_set_vel_ += 0.1;
        //display
        ui->lcdNumber_set_vel->display(QString::number(m_set_vel_, 'f', 2));
    }
}

void MainWindow::on_button_down_clicked()
{
    if(ui->checkBox_rover_enable->checkState() == Qt::Unchecked)
        return;
    if(m_set_vel_ > -1.0)
    {
        m_set_vel_ -= 0.1;
        // display
        ui->lcdNumber_set_vel->display(QString::number(m_set_vel_, 'f', 2));
    }
}

void MainWindow::on_button_left_clicked()
{
    if(ui->checkBox_rover_enable->checkState() == Qt::Unchecked)
        return;
    if(m_set_angle_ < 1.0)
    {
        m_set_angle_ += 0.1;
        //display
        ui->lcdNumber_set_ang->display(QString::number(m_set_angle_, 'f', 2));
    }
}

void MainWindow::on_button_right_clicked()
{
    if(ui->checkBox_rover_enable->checkState() == Qt::Unchecked)
        return;
    if(m_set_angle_ > -1.0)
    {
        m_set_angle_ -= 0.1;
        //display
        ui->lcdNumber_set_ang->display(QString::number(m_set_angle_, 'f', 2));
    }
}
void MainWindow::on_button_reset_clicked()
{
    if(ui->checkBox_rover_enable->checkState() == Qt::Unchecked)
        return;
    
    reset_twist();
}

void MainWindow::reset_twist()
{  
    m_set_vel_ = 0.0;
    m_set_angle_ = 0.0;

    ui->lcdNumber_set_ang->display(QString::number(m_set_angle_, 'f', 2));
    ui->lcdNumber_set_vel->display(QString::number(m_set_vel_, 'f', 2));
}

void MainWindow::onStateChanged(int state)
{
    if(!ros_flag_){
        QMessageBox::information(this, "warning!!!", "please connect roscore.");
        return;
    }
    if (state == Qt::Checked) // "选中"
    {
        qDebug()<< "check";
        setControlMode(1);
    }
    else if(state == Qt::Unchecked) // 
    {
        qDebug()<< "uncheck";
        setControlMode(0);
        reset_twist();
        setControlData(0.0,0.0,true);
    }

}

void MainWindow::imuCallback(const sensor_msgs::Imu::ConstPtr &msg)
{
    static int cnt = 0;
    //降低频率
    if(cnt > 10){
        double time1 = msg->header.stamp.toSec();
        QString strValue = QString::number(time1, 'f', 4);
        double accx,accy,accz,gryx,gryy,gryz;
        gryx = msg->angular_velocity.x;
        gryy = msg->angular_velocity.y;
        gryz = msg->angular_velocity.z;

        accx = msg->linear_acceleration.x;
        accy = msg->linear_acceleration.y;
        accz = msg->linear_acceleration.z;

        ui->lcdNumber_imuTime->display(strValue);
        ui->lcdNumber_accx->display(QString::number(accx, 'f', 4));
        ui->lcdNumber_accy->display(QString::number(accy, 'f', 4));
        ui->lcdNumber_accz->display(QString::number(accz, 'f', 4));
        ui->lcdNumber_gryx->display(QString::number(gryx, 'f', 4));
        ui->lcdNumber_gryy->display(QString::number(gryy, 'f', 4));
        ui->lcdNumber_gryz->display(QString::number(gryz, 'f', 4));

        cnt = 0;
    }
    cnt ++;
}

void MainWindow::lidarCallback(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
    if(msg){
        pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_pcd(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::fromROSMsg(*msg, *tmp_pcd);
        double time1 = msg->header.stamp.toSec();
        int size = tmp_pcd->points.size();
        ui->lcdNumber_lidarTime->display(QString::number(time1, 'f', 4));
        ui->lcdNumber_lidar_pointSize->display(QString::number(size));

        //点云display
        m_point_cloud_viewer_->clearPointCloud();
        m_point_cloud_viewer_->setPointCloud(tmp_pcd);
    }
}

void MainWindow::callbackNavTwistRaw(const geometry_msgs::TwistStampedConstPtr &msg)
{
    if(msg){
        double vel, angle;

        vel = msg->twist.linear.x;
        angle = msg->twist.angular.z;
        ui->lcdNumber_cur_vel->display(QString::number(vel, 'f', 2));
        ui->lcdNumber_cur_ang->display(QString::number(angle, 'f', 2));
    }
}

void MainWindow::setControlMode(int mode)
{
    if(!ros_flag_)
        return;
    std::string switch_mode = "swith_nav";
    (mode == 0) ? (switch_mode = "swith_nav") : (switch_mode = "swith_manual");
    std_msgs::String msg_str;
    msg_str.data = switch_mode;
    control_mode_pub_.publish(msg_str);
}

void MainWindow::setControlData(const float vel, const float angle, bool flag)
{
    if(!ros_flag_)
        return;
    if(ui->checkBox_rover_enable->checkState() == Qt::Unchecked && !flag)
        return;
    geometry_msgs::TwistStamped manual_ts;
    manual_ts.header.stamp = ros::Time::now();
    manual_ts.header.frame_id = "manual";
    manual_ts.twist.linear.x = vel;
    manual_ts.twist.angular.z = angle;
    control_pub_.publish(manual_ts);
}

void MainWindow::initUI()
{
    ui->tabWidget->setCurrentIndex(0);
    ui->lcdNumber_imuTime->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_imuTime->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_imuTime->setSmallDecimalPoint(true);
    ui->lcdNumber_imuTime->setDigitCount(18);

    ui->lcdNumber_accx->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_accx->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_accx->setSmallDecimalPoint(true);
    ui->lcdNumber_accx->setDigitCount(18);

    ui->lcdNumber_accy->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_accy->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_accy->setSmallDecimalPoint(true);
    ui->lcdNumber_accy->setDigitCount(18);

    ui->lcdNumber_accz->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_accz->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_accz->setSmallDecimalPoint(true);
    ui->lcdNumber_accz->setDigitCount(18);

    ui->lcdNumber_gryx->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_gryx->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_gryx->setSmallDecimalPoint(true);
    ui->lcdNumber_gryx->setDigitCount(18);

    ui->lcdNumber_gryy->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_gryy->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_gryy->setSmallDecimalPoint(true);
    ui->lcdNumber_gryy->setDigitCount(18);

    ui->lcdNumber_gryz->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_gryz->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_gryz->setSmallDecimalPoint(true);
    ui->lcdNumber_gryz->setDigitCount(18);

    ui->lcdNumber_lidarTime->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_lidarTime->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_lidarTime->setSmallDecimalPoint(true);
    ui->lcdNumber_lidarTime->setDigitCount(18);

    ui->lcdNumber_lidar_pointSize->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_lidar_pointSize->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_lidar_pointSize->setSmallDecimalPoint(true);
    ui->lcdNumber_lidar_pointSize->setDigitCount(18);

    ui->lcdNumber_cur_vel->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_cur_vel->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_cur_vel->setSmallDecimalPoint(true);
    ui->lcdNumber_cur_vel->setDigitCount(5);

    ui->lcdNumber_cur_ang->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_cur_ang->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_cur_ang->setSmallDecimalPoint(true);
    ui->lcdNumber_cur_ang->setDigitCount(5);

    ui->lcdNumber_set_vel->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_set_vel->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_set_vel->setSmallDecimalPoint(true);
    ui->lcdNumber_set_vel->setDigitCount(5);

    ui->lcdNumber_set_ang->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdNumber_set_ang->setStyleSheet("QLCDNumber {background-color:white;color:red}"); //背景透明 设置字体色

    ui->lcdNumber_set_ang->setSmallDecimalPoint(true);
    ui->lcdNumber_set_ang->setDigitCount(5);
}